/**
 * MUL8.cpp
 *
 *  Created on: Nov 13, 2013
 *      Author: Kellen Carey
 *
 *  Edited 12/30/2013 by Adam Stroud
 *  Added Instance of BatteryMonitor
 */

#include "MUL8.h"
#include "MotorController.h"
#include "Vision.h"
#include "Android_Control.h"
//#include "InertialData.h"
//#include "BatteryMonitor.h"
#include <stdio.h>
#include <time.h>

MUL8::MUL8() {


}

MUL8::~MUL8() {

}

void MUL8::run() {
	// Create an instance of MotorController, with which we can perform different motions

	MotorController motorController;
	Android_Control android_control;
	Vision vision;

/* this is for testing. without it, mul8 will remain limp until a command is issued*/
	motorController.init();
	motorController.setMotion("Stand");

	vision.init(motorController);

	int step_result;
	std::string command;
	int visionOccupier;
	String commandName;
	bool visionOFF= true;
	bool androidON= true;
	visionOFF= true; //true= vision will not run until told. false= runs by default

	// Infinite loop

	//TODO Press enter. robot will stand. press enter again. robot will track ball
	//	getchar();
	//	motorController.setMotion("Stand");// We need to update the current motion from whatever vision.step() has returned
	//
	//	clock_t startTime = clock();
	//
	//	while ((((float)clock() - (float)startTime)/CLOCKS_PER_SEC) < 7) {
	//		motorController.step(false);
	//	}
	//	getchar();



	while (true)
	{
		// The following false's will eventually be replaced by inertialData.step()
		// Keep trying to find the ball, I believe each step analyzes one image frame

		if(!visionOFF){
			vision.nextFrame();// vision only runs when visionOFF= false

		}

		//std::cout<<"result of vision: "<<visionOccupier<<"\n"<<std::endl;

		/**
		 * comment out the following block if you wish to test without Android control
		 */
		command = android_control.step(androidON);

		if(command== "blank"){

		}
		else if(command=="stand"){
			visionOFF= true;
			commandName= "stand";

			std::cout << "MUL8 Command: "<< commandName<<"\n"<< std::endl;

			motorController.setMotion("Stand");// We need to update the current motion from whatever vision.step() has returned

			clock_t startTime = clock();

			while ((((float)clock() - (float)startTime)/CLOCKS_PER_SEC) < 6) {
				step_result = motorController.step(false);
			}

		}
		//			if(command==2){
		//
		//				commandName= "Walk";
		//
		//				std::cout << "MUL8 Command: "<< commandName<<"\n"<< std::endl;
		//
		//				motorController.setMotion("Stand");// We need to update the current motion from whatever vision.step() has returned
		//
		//				clock_t startTime = clock();
		//
		//				while ((((float)clock() - (float)startTime)/CLOCKS_PER_SEC) < 7) {
		//					motorController.step(false);
		//				}
		//			}
		else if(command=="kick"){
			visionOFF= true;
			commandName= "kick";

			std::cout << "MUL8 Command: "<< commandName<<"\n"<< std::endl;

			motorController.setMotion("KickL");// We need to update the current motion from whatever vision.step() has returned

			clock_t startTime = clock();

			while ((((float)clock() - (float)startTime)/CLOCKS_PER_SEC) < 7) {
				step_result = motorController.step(false);
			}

		}
		else if(command=="relax"){
			visionOFF= true;
			commandName= "relax";

			std::cout << "MUL8 Command: "<< commandName<<"\n"<< std::endl;

			motorController.disableAllMotors();// We need to update the current motion from whatever vision.step() has returned
		}


		else if(command=="engage"){
			visionOFF= true;
			commandName= "engage";

			std::cout << "MUL8 Command: "<< commandName<<"\n"<< std::endl;

			motorController.enableAllMotors();// We need to update the current motion from whatever vision.step() has returned
		}


		/* When 'Track' is recieved from android, vision.step begins begins*/
		else if(command=="track"){
			commandName= "Track Ball";
			std::cout << "MUL8 Command: "<< commandName<<"\n"<< std::endl;
			visionOFF= false;
			vision.setAction(CENTER_BALL);
			androidON=true;

		}






		/**
		 * comment the following 6 lines out if you wish to test without full body motions
		 */



	}

	// Should never get here
	return;



}
